import struct
import serial
import time
import threading
import json
from config.config import MotorCfg

class MotorResponse:
    def __init__(self, data):
        """
        初始化
        :param data: 响应数据
        """
        
        self.head = None # 头部信息
        
        self.cmd = None # 命令
        
        self.motor_id = None # 电机ID
        
        self.data_size = None # 数据大小
        
        self.data = None # 数据
        
        self.cmd_sum = None # 命令校验和
        
        self.data_sum = None # 数据校验和
        
        self.is_valid = True # 是否有效
        
        self._parse_response(data)

    def _calculate_checksum(self, data):
        """
        计算字节数组的校验和
        :param data: 字节数据
        :return: 校验和
        """
        checksum = sum(data) & 0xFF
        return checksum
    def _parse_response(self, data):
        """
        解析响应数据
        :param response: 响应数据
        :return: 解析结果
        """
        if len(data) < 5:
            print("响应数据长度不足")
            return
        self.head = data[0]
        self.cmd = data[1]
        self.motor_id = data[2]
        self.data_size = data[3]
        self.cmd_sum = self._calculate_checksum(data[:4]) 
        self.data = data[5:5 + self.data_size] if self.data_size > 0 else None
        # if self.cmd_sum != data[4]:
        #     self.is_valid = False
        #     print("校验和错误")
        #     return
        # if self.data_size > 0:
        #     data_sum = self._calculate_checksum(data[5:5 + self.data_size])
        #     if data_sum != data[5 + self.data_size]:
        #         self.is_valid = False
        #         print("校验和错误")
        #         return

class MotorHead:
    def __init__(self, port, baudrate=115200):
        """
        初始化电机控制类，设置串口通信参数。
        :param port: 串口端口（例如 COM8）
        :param baudrate: 波特率（默认115200）
        """
        self.mutex = threading.Lock()
        
        self.ser = serial.Serial(
            port=port,                 # 串口端口（根据实际情况设置）
            baudrate=baudrate,         # 波特率
            bytesize=8,                # 数据位：8位
            parity='N',                # 校验位：无
            stopbits=serial.STOPBITS_ONE, # 停止位：1位
            timeout=1                  # 读取超时：1秒
        )

    def send_data(self, hex_data):
        """
        发送十六进制数据到电机
        :param hex_data: 发送的十六进制数据
        """
        try:
            self.mutex.acquire()
            if self.ser.is_open:
                self.ser.write(hex_data)
                # print(f"发送的十六进制数据: {hex_data.hex().upper()}")
            else:
                print("串口未打开，无法发送数据")
            self.mutex.release()
            time.sleep(0.01)
        except Exception as e:
            print(f"发送数据时发生错误: {e}")
        

    def hex_string_to_array(self, hex_string):
        # 将16进制字符串按照每两个字符一组分割
        hex_pairs = [hex_string[i:i+2] for i in range(0, len(hex_string), 2)]
        
        # 将每组16进制字符串转换为十进制数值
        decimal_array = [int(pair, 16) for pair in hex_pairs]
        
        return decimal_array
    def read_response(self):
        """
        读取串口返回的数据
        :return: 返回的数据
        """
        if self.ser.is_open:
            data = self.ser.read(64)  # 读取最多64字节的返回数据
            print(f"接收到的响应数据: {data.hex().upper()}")
            data = self.hex_string_to_array(data.hex().upper())
            response = MotorResponse(data)
            # print(f"解析后的响应数据: {json.loads(response)}")
            return response
        else:
            print("串口未打开，无法读取数据")
            return None

    def motor_on(self, motor_id):
        """
        启动电机（将电机从关闭状态切换到开启状态）
        :param motor_id: 电机ID（1~32）
        """
        cmd = [0x3E, 0x88, motor_id, 0x00]
        cmd.append(self._calculate_checksum(cmd))  # 将校验和附加到命令的末尾
        self._send_command(cmd)

    def motor_stop(self, motor_id):
        """
        停止电机（但不清除电机运行状态）再次发送控制指令即可控制电机动作。
        :param motor_id: 电机ID（1~32）
        """
        cmd = [0x3E, 0x81, motor_id, 0x00]
        cmd.append(self._calculate_checksum(cmd))  # 将校验和附加到命令的末尾
        self._send_command(cmd)

    def motor_off(self, motor_id):
        """
        将电机从开启状态（上电后默认状态）切换到关闭状态，LED 由常亮转为慢闪。此时电机仍然可以回复命令，但不会执行动作
        :param motor_id: 电机ID（1~32）
        """
        cmd = [0x3E, 0x80, motor_id, 0x00]
        cmd.append(self._calculate_checksum(cmd))  # 将校验和附加到命令的末尾
        self._send_command(cmd)    

    def read_status(self, motor_id):
        """
        读取电机状态和错误标志
        :param motor_id: 电机ID（1~32）
        :return: 电机状态和错误标志
        """
        cmd = [0x3E, 0x9A, motor_id, 0x00]
        cmd.append(self._calculate_checksum(cmd))  # 将校验和附加到命令的末尾
        response = self._send_command_with_response(cmd)
        
        motor_id = response.motor_id

        temperature = response.data[0]
        voltage = (response.data[1] << 8) | response.data[2]
        current = (response.data[3] << 8) | response.data[4]
        
        # motorState = 0x00 电机处于开启状态； motorState = 0x10 电机处于关闭状态。
        motor_state = "ON" if response.data[5] == 0x00 else "OFF"
        """
        errorState 位 状态说明 0 1
        0 低电压状态 正常 低压保护
        1 高电压状态 正常 高压保护
        2 驱动温度状态 正常 驱动过温
        3 电机温度状态 正常 电机过温
        4 电机电流状态 正常 电机过流
        5 电机短路状态 正常 电机短路
        6 堵转状态 正常 电机堵转
        7 输入信号状态 正常 输入信号丢失超时
        """
        error_state = bin(response.data[6])[2:].zfill(8)

        return {
            "temperature": temperature,
            "voltage": voltage,
            "current": current,
            "motor_state": motor_state,
            "error_state": error_state
        }
            

    def clear_errors(self, motor_id):
        """
        清除电机错误状态
        :param motor_id: 电机ID（1~32）
        """
        cmd = [0x3E, 0x9B, motor_id, 0x00]
        cmd.append(self._calculate_checksum(cmd))  # 将校验和附加到命令的末尾
        self._send_command(cmd)

    def set_angle(self, motor_id, target_angle):
        """
        设置电机旋转到指定角度（单位：度）
        :param motor_id: 电机ID（1~32）
        :param target_angle: 目标角度（单位：度，0~360）
        :param max_speed: 最大速度（单位：度/秒）
        """
        spinDirection = 0X00 if target_angle > 0 else 0X01  # 根据目标角度确定转动方向
        target_angle = abs(target_angle)  # 将目标角度取绝对值

        angle_LSB = int(target_angle * 100)  # 将目标角度转换为整数，乘以100

        # 目标角度数据需要分两字节表示
        cmd = [0x3E, 0xA5, motor_id, 0x04]  # 命令字节，4字节数据
        cmd.append(self._calculate_checksum(cmd))  # 将校验和附加到命令的末尾
        
        # 数据帧：spinDirection + angleControl低字节 + angleControl高字节 + NULL字节
        data = [
            spinDirection,  # 转动方向
            angle_LSB & 0xFF,           # 位置控制字节1（低字节）
            (angle_LSB >> 8) & 0xFF,    # 位置控制字节2（高字节）
            0x00  # NULL字节
        ]
        data.append(self._calculate_checksum(data))  # 将校验和附加到数据的末尾
        full_command = cmd + data

        self._send_command(full_command)

    def __set_angle_increment(self, motor_id, increment_angle):
        """
        设置电机旋转到指定角度（单位：度）
        :param motor_id: 电机ID（1~32）
        :param increment_angle: 目标角度（单位：度，0~360）
        :param max_speed: 最大速度（单位：度/秒）
        """
        
        speed = int(7200000) #7200000

        angle_LSB = int(increment_angle * 100)

        # 目标角度数据需要分两字节表示
        cmd = [0x3E, 0xA8, motor_id, 0x08]  # 命令字节，4字节数据
        cmd.append(self._calculate_checksum(cmd))  # 将校验和附加到命令的末尾
                    # 数据帧：控制值 angleIncrement 为 int32_t 类型
        data = [
                angle_LSB & 0xFF,           # 增量位置控制低字节 1 
                (angle_LSB >> 8) & 0xFF,    # 增量位置控制字节 2
                (angle_LSB >> 16) & 0xFF,    # 增量位置控制字节 3
                (angle_LSB >> 24) & 0xFF,    # 增量位置控制高字节 4
                speed & 0xFF,           # 增量位置控制低字节 1 
                (speed >> 8) & 0xFF,    # 增量位置控制字节 2
                (speed >> 16) & 0xFF,    # 增量位置控制字节 3
                (speed >> 24) & 0xFF    # 增量位置控制高字节 4
        ]
        data.append(self._calculate_checksum(data))  # 将校验和附加到数据的末尾
        full_command = cmd + data
        self._send_command(full_command)
        

    def set_angle_increment(self, motor_id, increment_angle):
        self.__set_angle_increment(motor_id, increment_angle)
        # thread1 = threading.Thread(target=self.__set_angle_increment, args=(motor_id, increment_angle))
        # thread1.start()
    
    def _send_command(self, cmd):
        """
        发送控制命令并返回电机响应
        :param cmd: 命令字节数组
        """
        
        checksum = self._calculate_checksum(cmd)  # 计算校验和
        cmd.append(checksum)                      # 将校验和附加到命令的末尾
        self.send_data(bytes(cmd))

    def _send_command_with_response(self, cmd):
        """
        发送控制命令并返回电机响应
        :param cmd: 命令字节数组
        """
        
        checksum = self._calculate_checksum(cmd)  # 计算校验和
        cmd.append(checksum)                      # 将校验和附加到命令的末尾
        self.send_data(bytes(cmd))
        response = self.read_response()
        
        return response

    def _int_to_bytes(self, value):
        """
        将整数转换为字节（4字节）
        :param value: 整数值
        :return: 转换后的字节数组（小端字节序）
        """
        return [value & 0xFF, (value >> 8) & 0xFF]

    def _calculate_checksum(self, data):
        """
        计算字节数组的校验和
        :param data: 字节数据
        :return: 校验和
        """
        checksum = sum(data) & 0xFF
        return checksum

    def close(self):
        """关闭串口"""
        if self.ser.is_open:
            self.ser.close()
            print("串口已关闭")
        else:
            print("串口未打开")

    def set_zero(self, motor_id):
        """
        主机发送该命令以设置电机的当前位置作为零点，同时清除圈数信息
        """
        cmd = [0x3E, 0x95, motor_id, 0x00]
        cmd.append(self._calculate_checksum(cmd))
        self._send_command_with_response(cmd)

    def set_zero_rom(self, motor_id):
        """
        主机发送该命令以设置电机的当前位置作为零点，同时清除圈数信息
        """
        cmd = [0x3E, 0x19, motor_id, 0x00]
        cmd.append(self._calculate_checksum(cmd))
        self._send_command_with_response(cmd)

    def read_current_angle(self, motor_id):
        """
        读取当前电机角度（单圈）
        :param motor_id: 电机ID（1~32）
        :return: 当前角度（单位：度）
        """
        cmd = [0x3E, 0x94, motor_id, 0x00]
        cmd.append(self._calculate_checksum(cmd))
        response = self._send_command_with_response(cmd)
        # response = self.read_response()
        if response is None:
            return None
        if not response.is_valid:
            return None
        if response.data is None:
            return None
        data = bytes(response.data[:4])
        # print(f"response.data: {data}")
        circle_angle = struct.unpack('<I', data)[0]
        # circle_angle = (response.data[1] << 24) | (response.data[2] << 16) | (response.data[3] << 8) | response.data[0]
        circle_angle = (circle_angle & 0x00FFFFFF) * 0.01  # 转换为度数
        return circle_angle

# region examples
def demo1():
    # 初始化电机对象
    motor = MotorHead(port='COM9')

    try:
        # 启动电机ID为1
        motor.motor_on(1)
        motor.motor_on(2)
        time.sleep(2)  # 等待2秒

        # 设置电机ID为1，旋转到指定角度 180度，最大速度为100度/秒
        motor.set_angle(motor_id = 1, target_angle = 0)
        motor.set_angle(motor_id = 2, target_angle = 180)
        time.sleep(5)  # 等待电机旋转完成
        current_angle = motor.read_current_angle(motor_id = 1)
        print(f"motor_id：{1}，当前角度：{current_angle}度")
        time.sleep(5)
        current_angle = motor.read_current_angle(motor_id = 2)
        print(f"motor_id：{2}，当前角度：{current_angle}度")
        # 停止电机ID为1
        motor.motor_off(1)
        motor.motor_off(2)
    finally:
        motor.close()  # 关闭串口

def demo2():   
# 初始化电机对象
    motor = MotorHead(port='COM10')
    try:
        # 启动电机ID为1
        # motor.motor_on(2)
        # motor.motor_on(3)
        motor.clear_errors(2)
        motor.clear_errors(3)
        # motor.set_zero_rom(2)
        # motor.set_zero_rom(3)
        target_angle = -10
        # max_angle = 50
        # target_angle = target_angle if target_angle < max_angle else max_angle
        motor2_angle = -target_angle
        motor3_angle = target_angle

        # current_motor2_angle = motor.read_current_angle(motor_id = 2)
        # print(f"motor_id：{2}，当前角度：{current_motor2_angle}度")
        # current_motor3_angle = motor.read_current_angle(motor_id = 3)
        # print(f"motor_id：{3}，当前角度：{current_motor3_angle}度")

        # 设置电机ID为1，旋转到指定角度 180度，最大速度为100度/秒
        motor.set_angle_increment(motor_id = 2, increment_angle = motor2_angle)
        motor.set_angle_increment(motor_id = 3, increment_angle = motor3_angle)
        # time.sleep(2)  # 等待电机旋转完成
        # current_angle = motor.read_current_angle(motor_id = 2)
        # print(f"motor_id：{2}，当前角度：{current_angle}度")
        # current_angle = motor.read_current_angle(motor_id = 3)
        # print(f"motor_id：{3}，当前角度：{current_angle}度")
        # motor.motor_off(2)
        # motor.motor_off(3)
    finally:
        motor.close()  # 关闭串口

def demo3():
    motor = MotorHead(port='/dev/ttyUSB1')
    # 启动电机ID为1
    # motor.motor_on(1)
    current_angle = motor.read_current_angle(motor_id = 1)
    print(f"motor_id：{1}，当前角度：{current_angle}度")
    target_angle = 50
    motor.set_angle_increment(1, target_angle)
    current_angle = motor.read_current_angle(motor_id = 1)
    print(f"motor_id：{1}，当前角度：{current_angle}度")
# endregion

class Head:
    
    def __init__(self, port='/dev/ttyUSB0', baudrate=115200):
        allDisabled = not (MotorCfg.head.motor1.enabled or MotorCfg.head.motor2.enabled or MotorCfg.head.motor3.enabled)
        
        if allDisabled:
            print("MotorCfg: all head motors are disabled")
            return

        self.motor = MotorHead(port=port, baudrate=baudrate)

        for motor_id in range(1, len(MotorCfg.head) + 1):
            if MotorCfg.head[f"motor{motor_id}"].enabled:
                self.motor.motor_on(motor_id)
                print(f"head motor{motor_id} enabled")
                self.motor.set_angle(motor_id=motor_id, target_angle=0) # 归零
                # self.motor.set_angle_increment(motor_id, 0) # 抱闸
                print(f"head motor{motor_id} go zero")
    
    def up(self, angle=10):
        assert MotorCfg.head.motor2.enabled and MotorCfg.head.motor3.enabled, "head motor2 or motor3 is disabled"
        self.motor.set_angle_increment(2, angle)
        self.motor.set_angle_increment(3, angle)

    def down(self, angle=-10):
        assert MotorCfg.head.motor2.enabled and MotorCfg.head.motor3.enabled, "head motor2 or motor3 is disabled"
        self.motor.set_angle_increment(2, angle)
        self.motor.set_angle_increment(3, angle)

    def left(self, angle=-30):
        assert MotorCfg.head.motor1.enabled, "head motor1 is disabled"
        self.motor.set_angle_increment(1, angle)

    def right(self, angle=30):
        assert MotorCfg.head.motor1.enabled, "head motor1 is disabled"
        self.motor.set_angle_increment(1, angle)

# 示例用法
if __name__ == "__main__":
    # demo3()
    head = Head(port='/dev/ttyUSB0')
    head.left()